#include "modules/MonitorModule/SmallObsMonitor.h"
#include "data_struct/base/DataSet.h"
#include "modules/CtrlModule/PassivePlanner/SmallObsPlanner.h"
#include "Communication/InterruptAndRecoveryCom.h"

namespace behavior_controller
{
#define OBS_DISTANCE 0.05 //沿边与小障碍物的距离
#define CHECk_DISTANCE (ROBOT_DIAMETER+4*OBS_DISTANCE)  // 1个扫地机直径距离+2倍沿相障碍物的直径
SmallObsMonitor small_obs_monitor;

void SmallObsMonitor::run()
{
    if (!inited)
    {
        get_area();
    }
    bool result = check_small_obs();
    if (small_obs_Planner.finished)
    {
        if (result)
        {
            bool seen = false;
            PointF obs_pose_world = DataSet::robot_pose.ConvertOnCur(small_obs_Planner.mean_pose_local);
            if (small_obs_Planner.small_obs.size() > 0)
            {
                for (int i = 0; i < small_obs_Planner.small_obs.size(); i++)
                {
                    if (small_obs_Planner.small_obs[i].second.Distance(obs_pose_world) < 2 * ROBOT_RADIUS
                        && small_obs_Planner.small_obs[i].first)
                    {
                        seen = true;
                        break;
                    }
                }
            }
            if (!seen)
            {
                small_obs_Planner.small_obs.push_back(std::make_pair(false, obs_pose_world));
                small_obs_Planner.finished = false;
                double length = 0.2;
                geometry_msgs::msg::PoseStamped pose = geometry_msgs::msg::PoseStamped();
                if (DataSet::ctrl_state == NAVI_ACTION && DataSet::ctrl_state_sub == NAVI_TO_POSE_PLAN)
                {
                    if (insert_pose_client.async_send_request(std::string("SmallObsPlanner"), length, true, pose))
                    {
                        std::cout << "SmallObsPlanner: pause navigation!" << std::endl;
                    }
                }
                updateState(-1, -1, -1, DataSet::clean_mode, DataSet::ctrl_state, DataSet::ctrl_state_sub);
                DataSet::ctrl_state = SMALL_OBS_PLAN;
                std::cout << "small_obs find !" << std::endl;
            }
        }
    } else
    {
        DataSet::ctrl_state = SMALL_OBS_PLAN;
    }
}

void SmallObsMonitor::get_area()
{
    //double neg_x, neg_y;
    double side_len;
    std::cout << "modify contour area !" << std::endl;
    nwe_area = contour_areas.front_area();

    for (int i = 0; i < nwe_area.contour_points().size(); i++)
    {
        if (nwe_area.contour_points()[i].GetPoint()(0) != 0)
        {
            side_len = fabs(nwe_area.contour_points()[i].GetPoint()(0));
            break;
        }
    }
    nwe_area_side_len = side_len + CHECk_DISTANCE;
    nwe_area.resize(4);
    nwe_area.ModifyContour(0, PointF(nwe_area_side_len, nwe_area_side_len));
    nwe_area.ModifyContour(1, PointF(nwe_area_side_len, -nwe_area_side_len));
    nwe_area.ModifyContour(2, PointF(-nwe_area_side_len, -nwe_area_side_len));
    nwe_area.ModifyContour(3, PointF(-nwe_area_side_len, nwe_area_side_len));
    std::cout << "nwe_area:size= " << nwe_area.contour_points().size() << " side_len= " << nwe_area_side_len
              << std::endl;
    inited = true;
}

bool SmallObsMonitor::check_small_obs()
{
    //获得的激光雷达数据为  激光雷达坐标系已经转化到机器人坐标系
    PointCloud<double> local_point_cloud;
    double meanx = 0, meany = 0, mean_distance = -1;
    int point_count = 0;

    for (auto pt : DataSet::scan_point_cloud)
    {
        PointF local_point(PoseF(pt, 0).GetPoint());
        if (fabs(local_point.GetPoint()(0)) <= nwe_area_side_len ||
            fabs(local_point.GetPoint()(1)) <= nwe_area_side_len)
        {
            if (nwe_area.IsPointInShape(local_point)) //判断点是否在轮廓内
            {
                if (local_point_cloud.size() > 0)
                {
                    if (local_point_cloud[local_point_cloud.size() - 1].DistanceSquare(local_point) > 0.01)
                        break;
                    local_point_cloud.emplace_back(local_point);
                } else
                    local_point_cloud.emplace_back(local_point);
            }
        }
    }
    point_count = local_point_cloud.size();
    small_obs_Planner.point_count = point_count;
    if (point_count > 0)
    {
        small_obs_Planner.obs_len = local_point_cloud[0].Distance(local_point_cloud[point_count - 1]);
        for (int i = 0; i < point_count - 1; i++)
        {
            meanx += local_point_cloud[i].GetPoint()(0);
            meany += local_point_cloud[i].GetPoint()(1);
        }
        double offset = 0.01;
        meanx = meanx / point_count;
        meany = meany / point_count;
        small_obs_Planner.obs_dis = std::hypot(meanx, meany);
        small_obs_Planner.turn_radius = small_obs_Planner.obs_len * 0.5 + small_obs_Planner.obs_dis +
                                        offset; // 使用obs_len作为小障碍物的直径计算出的转弯半径，实际效果看有点小，应给与补偿合适的补偿offset.
        small_obs_Planner.mean_pose_local = PointF(meanx, meany);
        mean_distance = small_obs_Planner.turn_radius;
    } else
    {
        small_obs_Planner.obs_len = -1;
        small_obs_Planner.turn_radius = -1;
        small_obs_Planner.obs_dis = -1;
    }
    //std::cout<< "point_count="<< point_count<< " mean_distance="<< mean_distance<< " obs_len="<< small_obs_Planner.obs_len<<std::endl;

    if (point_count < 20 && point_count >= 3 && mean_distance <= (ROBOT_RADIUS + 0.05))
    {
        std::cout << "point_count=" << point_count << " mean_distance=" << mean_distance << " obs_len="
                  << small_obs_Planner.obs_len << std::endl;
        return true;
    }
    return false;
}


}